import math

from soccerbots.core.structs import *

def torad(angle):
    return (angle*math.pi)/180

def togra(angle):
    return int( ((angle*180)/math.pi)%360 )

def dcos(angle):
    return math.cos(torad(angle))

def dsin(angle):
    return math.sin(torad(angle))

class Phisics:
    def __init__(self):
        self.parets = None
        self.state = None
        self.fregament = 200
        self.ball_r = 8
        self.robot_r = 37
        self.robot_m = 45
        self.robot_p = 50
        self.robot_d = 52

    def compute(self, gamestate, time, relocate=0):
        if relocate==0:
            self.state = gamestate
            self.time = time
            self.moveBall()
            self.moveRobots()

    def moveBall(self):
        ball = self.state.ball
        ball.linearVel -= self.fregament * self.time
        if ball.linearVel < 0: ball.linearVel = 0
        vel = self.getComponents(ball)
        self.applyVelocity(ball, vel)

        # Rebots amb parets
        r = self.ball_r+2
        if abs(ball.position[0])>1100 and abs(ball.position[1]-900)<(200+r):
            if (abs(ball.position[1]-900)+r)>200:
                ball.direction = 360-ball.direction
            if (abs(ball.position[0])+r)>1250:
                ball.direction = 180-ball.direction
        else:
            if (abs(ball.position[1]-900)+r)>900:
                ball.direction = 360-ball.direction
            if (abs(ball.position[0])+r)>1100:
                ball.direction = 180-ball.direction

        #rebots amb robots
        robot,dist = self.getNearestRobot(ball)
        if dist < self.robot_m:
            dp = ball.direction - (robot.direction + 45)
            if (math.sin(dp)*math.cos(dp))>0:
                #horz
                ball.direction = 360-ball.direction
            else:
                #vert
                ball.direction = 180-ball.direction
            rvel = self.getComponents(robot)
            bvel = self.getComponents(ball)
            nvel = (rvel[0]+bvel[0], rvel[1]+bvel[1])
            ball.linearVel = int(self.composeVelocity(nvel))
            ball.direction = self.composeDirection(nvel)
            ball.moguda = True
        else:
            ball.moguda = False

        ball.direction %= 360

    def moveRobots(self):
        for team in [self.state.teamA,self.state.teamB]:
            for player in TeamState.keyset:
                team[player].nom = player
                self.moveRobot(team[player])

    def moveRobot(self, robot):
        self.applyAngular(robot)

        if robot.linearVel == 0: return

        # Rebots amb robots
        near,dist = self.getNearestRobot(robot)
        if dist < (self.robot_p<<1):
            if robot.linearVel < 0:
                direction = (robot.direction+180)%360
                kk = -1
            else:
                direction = robot.direction
                kk = 1
            
            a = near.position[0] - robot.position[0]*kk
            b = near.position[1] - robot.position[1]*kk
            angle = togra(math.atan(b/a))
            if a<0: angle = angle+180
            angle %= 360

            if abs(angle-direction)<90:
                robot.linearVel = 0
        if robot.linearVel == 0: return

        # Rebots amb parets
        r = self.robot_m
        rdirection = robot.direction % 360
        if robot.linearVel<0:
            rdirection += 180
            rdirection %= 360
        
        if ((robot.position[1]-900)+r)>900 and rdirection<180:
            robot.linearVel = 0
            
        if ((robot.position[1]-900)-r)<-900 and rdirection>180:
            robot.linearVel = 0
        
        if (robot.position[0]-r)<-1100 and (rdirection-90)<180:
            robot.linearVel = 0

        if (robot.position[0]+r)>1100 and (rdirection>270 or rdirection<90):
            robot.linearVel = 0
        
        if robot.linearVel == 0: return
        
        # Accelerar robot
        vel = self.getComponents(robot)
        self.applyVelocity(robot, vel)
        
        # Xutar pilota
        dist = self.getDist(robot, self.state.ball)
        if dist < r and not self.state.ball.moguda:
            ball = self.state.ball
            self.applyVelocity(ball, vel)
            bvel = self.getComponents(ball)
            nvel = (vel[0]+bvel[0], vel[1]+bvel[1])
            ball.linearVel = int(self.composeVelocity(nvel))
            ball.direction = self.composeDirection(nvel)

    def getNearestRobot(self, object):
        near = None
        dist = 10000
        for team in [self.state.teamA,self.state.teamB]:
            for player in TeamState.keyset:
                if object!=team[player]:
                    nd = self.getDist(object, team[player]) 
                    if nd < dist:
                        near = team[player]
                        dist = nd
        return near,dist

    def getNearRobots(self, object):
        near = []
        for team in [self.state.teamA,self.state.teamB]:
            for player in TeamState.keyset:
                if self.getDist(object, team[player]) < self.ball_r:
                    near.append(team[player])
        return near
            
    def getDist(self, a, b):
        x = math.pow(a.position[0]-b.position[0],2)
        y = math.pow(a.position[1]-b.position[1],2)
        return math.sqrt(x+y)

    def getComponents(self, obj):
        return (obj.linearVel*dcos(obj.direction), obj.linearVel*dsin(obj.direction))

    def decreaseVelocity(self, vel):
        x = vel[0] - self.fregament * self.time
        y = vel[1] - self.fregament * self.time
        return (x,y) 

    def applyVelocity(self, object, vel, factor=1):
        x = object.position[0] + vel[0] * self.time * factor
        y = object.position[1] + vel[1] * self.time * factor
        object.position = (x,y) 

    def composeVelocity(self, vel):
        return math.sqrt(math.pow(vel[0],2)+math.pow(vel[1],2))
    
    def composeDirection(self, vel):
        d = math.atan(vel[1]/max(vel[0],1))
        if vel[0]<0:
            d += math.pi
        return togra(d)

    def applyAngular(self, robot):
        robot.direction += robot.angVel * self.time
        robot.direction %= 360
